#include "Protocol.h"

// using namespace NS_SDK;

 //---虚拟点位发送---
bool Proto::DBG_Virtual_Pts_Set_20000_WaitFin()
{
    int DBG_Num_Pts = 1000;
    std::vector<tpDNRouteStruct> pts;

    pts.reserve(DBG_Num_Pts);
    
    auto result = _pc.Get_Pos();
    if(!result.first)
    {
        return false;
    }
    const tpDNPositionStruct cur_pos = result.second;

    for (int i = 0; i < DBG_Num_Pts; ++i)
    {
        // 1. 创建临时对象（每次循环生成新对象）
        tpDNRouteStruct tempRoute{
            .pos = {
            .x = cur_pos.x + (i+1) * 0.02,   //10s内走完全程，所有每10ms走20
            .y = cur_pos.y + (i+1) * 0.02,
            .z = cur_pos.z + (i+1) * 0.02,
            .rx = cur_pos.rx + (i+1) * 0.02,
            .rz = cur_pos.rz + (i+1) * 0.36,
            .ry = cur_pos.ry + (i+1) * 0.36  // 显式初始化保留字段
            },
            .vx = 200,
            .vy = 200,
            .vz = 200,
            .vrx = 200,
            .vry = 200,  // 显式初始化保留字段
            .vrz = 200,
            .ax = 10,
            .ay = 10,
            .az = 10,
            .arx = 10,
            .ary = 10,   // 显式初始化保留字段
            .arz = 10
            
        };

        pts.push_back(std::move(tempRoute));
    }

    return SetRoutePts(pts,DBG_Num_Pts,true);
}


 //---虚拟点位发送---
bool Proto::DBG_Virtual_Pts_Set_20000()
{
    int DBG_Num_Pts = 1000;
    std::vector<tpDNRouteStruct> pts;

    pts.reserve(DBG_Num_Pts);

    auto result = _pc.Get_Pos();
    if(!result.first)
    {
        return false;
    }
    const tpDNPositionStruct cur_pos = result.second;
    
    for (int i = 0; i < DBG_Num_Pts; ++i)
    {
        // 1. 创建临时对象（每次循环生成新对象）
        tpDNRouteStruct tempRoute{
            .pos = {
            .x = cur_pos.x + (i+1) * 0.02,   //10s内走完全程，所有每10ms走20
            .y = cur_pos.y + (i+1) * 0.02,
            .z = cur_pos.z + (i+1) * 0.02,
            .rx = cur_pos.rx + (i+1) * 0.02,
            .rz = cur_pos.rz + (i+1) * 0.360,
            .ry = cur_pos.ry + (i+1) * 0.360  // 显式初始化保留字段
            },
            .vx = 200,
            .vy = 200,
            .vz = 200,
            .vrx = 200,
            .vry = 200,  // 显式初始化保留字段
            .vrz = 200,
            .ax = 10,
            .ay = 10,
            .az = 10,
            .arx = 10,
            .ary = 10,   // 显式初始化保留字段
            .arz = 10
            
        };

        pts.push_back(std::move(tempRoute));
    }

   return SetRoutePts(pts,DBG_Num_Pts,false);
}

 //---虚拟点位发送---
bool Proto::DBG_Virtual_Pts_Set_0()
{
    int DBG_Num_Pts = 1000;
    std::vector<tpDNRouteStruct> pts;

    pts.reserve(DBG_Num_Pts);

    auto result = _pc.Get_Pos();
    if(!result.first)
    {
        return false;
    }
    const tpDNPositionStruct cur_pos = result.second;

    
    for (int i = 0; i < DBG_Num_Pts; ++i) {
        // 1. 创建临时对象（每次循环生成新对象）
        tpDNRouteStruct tempRoute{
            .pos = {
            .x = cur_pos.x  - (i+1) * 0.02,
            .y = cur_pos.y  - (i+1) * 0.02,
            .z = cur_pos.z  - (i+1) * 0.02,
            .rx = cur_pos.rx  - (i+1) * 0.02,
            .rz = cur_pos.rz  - (i+1) * 0.36,
            .ry = cur_pos.ry  - (i+1) * 0.36, // 显式初始化保留字段
            },
            .vx = 200,
            .vy = 200,
            .vz = 200,
            .vrx = 200,
            .vry = 200,  // 显式初始化保留字段
            .vrz = 200,
            .ax = 10,
            .ay = 10,
            .az = 10,
            .arx = 10,
            .ary = 10,   // 显式初始化保留字段
            .arz = 10
            
        };

        pts.push_back(std::move(tempRoute));
    }

    return SetRoutePts(pts,DBG_Num_Pts,false);
}

bool Proto::SetRoutePts(std::vector<tpDNRouteStruct> const & pos_routes,uint32_t num_pts,bool is_wait_run_fin)
{
    uint32_t filter_cnt = 0;
    bool ret = false;
    uint8_t run_stat = 0;
    tpDNPositionStruct init_pos;

    int DBG_xyz[6] = {0};
    float DBG_xyz_T[6] = {0};
    int idx = 0;

    if(num_pts > MAX_NUM_POS_PTS)  //大于最大允许规划点数
    {
        return false;
    }

    //存放路径点位的原始值 mm,degree
    route_origin = std::make_shared<std::vector<tpDNRouteStruct>>(pos_routes);  //触发深拷贝，确保调用者后续还可以访问操作 pos_routes 

    //存放路径点位的编码器单位的值
    route_pts = std::make_shared<std::vector<tpDNRouteStruct_Enc>>();

    if(!Is_Motion_Fin()) //获取是否之前的路径有无执行完成，同时获取当前的增编值和绝对值编码器值
    { return false;}

    if(_pc.Is_1st_GetPos())  //如果是第一次获取位置
    {
        init_pos = _pc.Refresh_Init_Pos(pos_rt_enc,abs_pos_rt_enc);  //获取运动前的起始位置
    }
    else
    {
        init_pos = _pc.Refresh_Rt_Pos(pos_rt_enc,abs_pos_rt_enc);  //获取运动前的起始位置
    }

    // _pc.Update_increment_enc_before_move(pos_rt_enc);
    

    for(const auto& elem : pos_routes) 
    {
        route_pts->push_back(tpDNRouteStruct_Enc
        {
            .pos = {
                //先计算出目标坐标值和当前坐标值的差值，换算成增编单位，再加上当前增编值。
                static_cast<int>( ((elem.pos.x - init_pos.x) * MM_2_ENC_HC) + pos_rt_enc[0] ), 
                static_cast<int>( ((elem.pos.y - init_pos.y) * MM_2_ENC_HC) + pos_rt_enc[1] ), 
                static_cast<int>( ((elem.pos.z - init_pos.z) * MM_2_ENC_HC) + pos_rt_enc[2] ), 
                static_cast<int>( ((elem.pos.z - init_pos.z) * MM_2_ENC_HC) + pos_rt_enc[3] ), 
                static_cast<int>( ((elem.pos.rx - init_pos.rx) * DEG_2_ENC_YY) + pos_rt_enc[4] ),  
                static_cast<int>( ((elem.pos.rz - init_pos.rz) * DEG_2_ENC_YY) + pos_rt_enc[5] )

            },  // 初始化pos结构体

            .vx = static_cast<int>(elem.vx * MM_2_ENC_HC), 
            .vy = static_cast<int>(elem.vy * MM_2_ENC_HC), 
            .vz = static_cast<int>(elem.vz * MM_2_ENC_HC),
            .vrx = static_cast<int>(elem.vrx * MM_2_ENC_HC), 
            .vry = static_cast<int>(elem.vry * DEG_2_ENC_YY), 
            .vrz = static_cast<int>(elem.vrz * DEG_2_ENC_YY),

            .ax = static_cast<int>(elem.ax * MM_2_ENC_HC), 
            .ay = static_cast<int>(elem.ay * MM_2_ENC_HC), 
            .az = static_cast<int>(elem.az * MM_2_ENC_HC),
            .arx = static_cast<int>(elem.arx * MM_2_ENC_HC), 
            .ary = static_cast<int>(elem.ary * DEG_2_ENC_YY),
            .arz = static_cast<int>(elem.arz * DEG_2_ENC_YY)
            
        });

        
        DBG_xyz_T[0] = elem.pos.x;
        DBG_xyz_T[1] = elem.pos.y;
        DBG_xyz_T[2] = elem.pos.z;
        DBG_xyz_T[3] = elem.pos.z;
        DBG_xyz_T[4] = elem.pos.rx;
        DBG_xyz_T[5] = elem.pos.rz;
        DBG_xyz[0] = static_cast<int>( ((elem.pos.x - init_pos.x) * MM_2_ENC_HC) + pos_rt_enc[0] );
        DBG_xyz[1] = static_cast<int>( ((elem.pos.y - init_pos.y) * MM_2_ENC_HC) + pos_rt_enc[1] );
        DBG_xyz[2] = static_cast<int>( ((elem.pos.z - init_pos.z) * MM_2_ENC_HC) + pos_rt_enc[2] );
        DBG_xyz[3] = static_cast<int>( ((elem.pos.z - init_pos.z) * MM_2_ENC_HC) + pos_rt_enc[3] );
        DBG_xyz[4] = static_cast<int>( ((elem.pos.rx - init_pos.rx) * DEG_2_ENC_YY) + pos_rt_enc[4] );
        DBG_xyz[5] = static_cast<int>( ((elem.pos.rz - init_pos.rz) * DEG_2_ENC_YY) + pos_rt_enc[5] );
        if((idx == 0) || (idx == 1) || (idx == 2))
        {
            std::cout << "*****xyz****mm_deg***: "<< DBG_xyz_T[0] <<", " << DBG_xyz_T[1] <<", "<< DBG_xyz_T[2] <<", "<< DBG_xyz_T[3] <<", "<<DBG_xyz_T[4] <<", "<<DBG_xyz_T[5] <<std::endl;
            std::cout << "*****xyz****enc***: "<< DBG_xyz[0] <<", " << DBG_xyz[1] <<", "<< DBG_xyz[2] <<", "<< DBG_xyz[3] <<", "<<DBG_xyz[4] <<", "<<DBG_xyz[5] <<std::endl;
        }
        idx++;

        //------DEBG-------------------
        
        // idx++;

        // vec_ptr->push_back(tpDNRouteStruct_Enc{
        //     .pos = {10, 20, 30, 45, 90, 0},  // 初始化pos结构体
        //     .vx = 100, .vy = 200, .vz = 300,
        //     .vrx = 5, .vry = 0, .vrz = 10,
        //     .ax = 50, .ay = 60, .az = 70,
        //     .arx = 2, .arz = 3, .ary = 0
        // });
        
    }

    pts_num_total = route_pts->size();

    is_pts_snd_timeout = false;   //pts发送超时标记清除
    is_pts_snd_finish = false;   //pts发送完成

    {
        std::lock_guard<std::mutex> lock(mutex_pts_snd);
        pts_idx = 0;
        flag_new_route = true;  //提示有一个新路径
    }
    cv_pts_snd.notify_one(); //通知开始发送点位


    //----------等待最终发送结果---------
    {
        std::unique_lock<std::mutex> lock(mutex_pts_snd_finish);
        cv_pts_snd_finsh.wait(lock, [this] { return is_pts_snd_finish; });     
    }

    if(is_pts_snd_timeout)
    {
        return false;  //发送超时，失败！
    }


    if(is_wait_run_fin)
    {
        ret = Cmd_StartRun(0);  //启动
        if(!ret)
        { 
            return false;
        }

        while(true)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));    
            if(Is_Motion_Fin())
            {
                break;
            }
        }
    }

    return true;  //发送成功。

}
    

void Proto::Event_Get_Ack()
{
    {
        std::lock_guard<std::mutex> lock(mutex_ack);
        flag_new_ack = true;
    }

    cv_ack.notify_one();
}

void Proto::Event_Ack_GetSetSpd()
{
    {
        std::lock_guard<std::mutex> lock(mutex_ack_spd);
        flag_new_spd_ack = true;
    }

    cv_ack_spd.notify_one();
}

void Proto::Event_Get_Rt_Infos()
{
    {
        std::lock_guard<std::mutex> lock(mutex_rt_infos);
        flag_rt_infos_ack = true;
    }

    cv_rt_infos.notify_one();
}


void Proto::Event_Get_SingleAix_Fin(uint8_t id,uint8_t stat)  //单个轴控制运动完成
{
    {
        std::lock_guard<std::mutex> lock(mutex_single[id-1]);
        flag_sigle_ack[id-1] = true;
        flag_sigle_stat[id-1] = stat;
    }

    cv_single[id-1].notify_one();
}

 //启动运行
bool Proto::Cmd_StartRun(uint16_t delay_trigger)
{
    uint8_t data[4] = {0};
    bool ret = false;

    data[0] = CMD_START;
    data[1] =  (uint8_t)((delay_trigger & 0xFF00) >> 8);
    data[2] =  (uint8_t)(delay_trigger & 0x00FF);

    ret = Send_Process(CMD_START,data,3,true);
    if(ret)
    {
        is_drv_motion_fin = false;
    }

    return ret;
}
    //暂停运行
bool Proto::Cmd_PauseRun()
{
    uint8_t data[2] = {0};

    data[0] = CMD_PAUSE;
    data[1] =  1;

    return Send_Process(CMD_PAUSE,data,2,true);
}

//继续运行
bool Proto::Cmd_ResumeRun()
{
    uint8_t data[2] = {0};

    data[0] = CMD_PAUSE;
    data[1] =  0;

    return Send_Process(CMD_PAUSE,data,2,true);
}

    //停止运行
bool Proto::Cmd_StopRun()
{
    uint8_t data[2] = {0};
    bool ret = false;

    data[0] = CMD_STOP;
    data[1] =  0;

    ret = Send_Process(CMD_STOP,data,2,true);
    if(ret)
    {
        is_drv_motion_fin = false;
    }

    return ret;
}

//获取实时信息
bool Proto::Cmd_Get_RT_Infos()
{
    uint8_t data[2] = {0};
    bool ret = false;

    data[0] = CMD_GET_RT_INFOS;
    data[1] =  0;

    ret = Send_Process(CMD_GET_RT_INFOS,data,2,false);
    if(!ret)
    { return false; }

    //-----等待 速度设置的 应答
    {
        std::unique_lock<std::mutex> lock(mutex_rt_infos);
        flag_rt_infos_ack = false;
        if (cv_rt_infos.wait_for(lock, std::chrono::milliseconds(200), [this]{ return flag_rt_infos_ack; })) 
        {
            flag_rt_infos_ack = false;
        } 
        else 
        {
            logger_->warn("Read RT infos time out !");
            std::cout << "[Warn] --- Read RT infos time out !" <<std::endl;
            return false;
        }
    }

    return true;

}

//设置DO 输出
bool Proto::Cmd_DO_Ctrl(uint8_t id,uint8_t on_off)
{
    uint8_t data[4] = {0};
    bool ret = false;

    if((id > DO4) || (id < DO1))
    { return false;}

    if((on_off != DO_ON) && (on_off != DO_OFF))
    { return false;}

    data[0] = CMD_SET_DO_OUTPUT;
    data[1] =  id;
    data[2] =  on_off;

    ret = Send_Process(CMD_SET_DO_OUTPUT,data,3,true);

    return ret;
}



 //设置默认速度
bool Proto::Cmd_Set_Default_Spd(uint8_t id,float spd, bool * succ)
{
    uint8_t data[8] = {0};
    int32_t spd_ori;
    uint32_t retry = 0;
    bool ret = false;

    if(id > 4)  //YY 电机
    {
        spd_ori = (int32_t)(spd * DEG_2_ENC_YY * 10);
    }
    else    //HC 电机
    {
        spd_ori = (int32_t)(spd * MM_2_ENC_HC * 10);
    }

    data[0] = CMD_SET_DEFAULT_SPD;
    data[1] =  id;
    data[2] =  (uint8_t)((spd_ori & 0xFF000000) >> 24);
    data[3] =  (uint8_t)((spd_ori & 0x00FF0000) >> 16);
    data[4] =  (uint8_t)((spd_ori & 0x0000FF00) >> 8);
    data[5] =  (uint8_t)(spd_ori & 0x000000FF);

    *succ = false;

    while(++retry < 5)
    {

        ret = Send_Process(CMD_SET_DEFAULT_SPD,data,6,false); 
        if(!ret)
        { return false; }

        logger_->info("Cmd_Set_Default_Spd  ,ID: {}",id);
        std::cout << "[INFO]:  Cmd_Set_Default_Spd  ,ID: "<< id <<std::endl;

        //-----等待 速度设置的 应答
        {
            std::unique_lock<std::mutex> lock(mutex_ack_spd);
            flag_new_spd_ack = false;
            if (cv_ack_spd.wait_for(lock, std::chrono::milliseconds(500), [this]{ return flag_new_spd_ack; })) 
            {
                flag_new_spd_ack = false;
                *succ = is_set_default_spd_ok[id-1]; 
                return true;
            } 
            else 
            {
                logger_->warn("Cmd_Set_Default_Spd  TIME OUT !");
                std::cout << "[WARN]:  Cmd_Set_Default_Spd  TIME OUT !" <<std::endl;
                return false;
            }
        }

    }

    return false;
}

//获取默认速度
bool Proto::Cmd_Get_Default_Spd(uint8_t id,float * spd,bool * succ)
{
    uint8_t data[2] = {0};
    // int32_t pos_ori;
    bool ret = false;

    data[0] = CMD_GET_DEFAULT_SPD;
    data[1] =  id;

    *succ = false;

    ret = Send_Process(CMD_GET_DEFAULT_SPD,data,2,false); 
    if(!ret)
    { return false; } 

    logger_->info("Read Default Spd ,ID: ",id);
    std::cout << "[INFO]:  Read Default Spd ,ID: "<< id <<std::endl;

    //-----等待 速度读取的 应答
    {
        std::unique_lock<std::mutex> lock(mutex_ack_spd);
        flag_new_spd_ack = false;
        if (cv_ack_spd.wait_for(lock, std::chrono::milliseconds(500), [this]{ return flag_new_spd_ack; })) 
        {
            flag_new_spd_ack = false;
            *succ = is_get_default_spd_ok[id-1]; 
            if(id > 4)  //YY 电机
            {
                *spd = (int32_t)(default_spd_Param[id-1] * ENC_2_DEG_YY);
            }
            else    //HC 电机
            {
                *spd = (int32_t)(default_spd_Param[id-1] * ENC_2_MM_HC);
            }
            // *spd = default_spd_Param[id-1] * 1.0f;
        } 
        else 
        {
            logger_->warn("Read Default Spd  TIME Out!");
            std::cout << "[WARN]:  Read Default Spd  TIME Out!" <<std::endl;
            return false;
        }
    }



    return true;
}

//控制单个轴运动
bool Proto::Cmd_Move_Sigle_Axis(uint8_t id,float pos,float spd)
{
    uint8_t data[12] = {0};
    int32_t pos_ori,spd_ori;
    bool ret;

    if(!Is_Motion_Fin()) //获取是否之前的路径有无执行完成，同时获取当前的增编值和绝对值编码器值
    { return false;}

    if(fabs(pos) < 0.00001f)   //距离太小
    { return false;}

    //判断速度和位置的符号是否一致
    if(pos > 0)
    {
        if(spd <= 0)
        {
             return false;
        }
    }
    else
    {
        if(spd >= 0)
        {
             return false;
        }
    }

    if(id > 4)  //YY 电机
    {
        if(spd > (RATED_SPD_RPS_YY * DEG_PER_ROUND_YY))
        {
            spd = RATED_SPD_RPS_YY * DEG_PER_ROUND_YY;
        }
        else if(spd < (-RATED_SPD_RPS_YY * DEG_PER_ROUND_YY))
        {
            spd = -RATED_SPD_RPS_YY * DEG_PER_ROUND_YY;
        }

        spd_ori = (int32_t)(spd * DEG_2_ENC_YY * 100);
        pos_ori = (int32_t)(pos * DEG_2_ENC_YY * 100);
    }
    else    //HC 电机
    {
        if(spd > (RATED_SPD_RPS_HC * MM_PER_ROUND_HC))
        {
            spd = RATED_SPD_RPS_HC * MM_PER_ROUND_HC;
        }
        else if(spd < (-RATED_SPD_RPS_HC * MM_PER_ROUND_HC))
        {
            spd = -RATED_SPD_RPS_HC * MM_PER_ROUND_HC;
        }

        spd_ori = (int32_t)(spd * MM_2_ENC_HC * 1000);
        pos_ori = (int32_t)(pos * MM_2_ENC_HC * 1000);
    }

    // pos_ori = (int32_t)(pos * 10);
    // spd_ori = (int32_t)(spd * 10);

    data[0] = CMD_CTRL_AIX;
    data[1] =  id;
    data[2] =  (uint8_t)((pos_ori & 0xFF000000) >> 24);
    data[3] =  (uint8_t)((pos_ori & 0x00FF0000) >> 16);
    data[4] =  (uint8_t)((pos_ori & 0x0000FF00) >> 8);
    data[5] =  (uint8_t)(pos_ori & 0x000000FF);
    data[6] =  (uint8_t)((spd_ori & 0xFF000000) >> 24);
    data[7] =  (uint8_t)((spd_ori & 0x00FF0000) >> 16);
    data[8] =  (uint8_t)((spd_ori & 0x0000FF00) >> 8);
    data[9] =  (uint8_t)(spd_ori & 0x000000FF);

    ret = Send_Process(CMD_CTRL_AIX,data,10,false);
    if(!ret)
    { return false; }

    int32_t waittime = 2 * ((int32_t)((pos/spd)) + 0.5f) *1000;  //单位是ms   四舍五入后*2倍

    // int32_t waittime = 60000;  //单位是ms   for DBG

    //-----等待 单轴控制完成 应答
    {
        std::unique_lock<std::mutex> lock(mutex_single[id-1]);
        flag_sigle_ack[id-1] = false;
        if (cv_single[id-1].wait_for(lock, std::chrono::milliseconds(waittime), [this,id]{ return flag_sigle_ack[id-1]; })) 
        {
            flag_sigle_ack[id-1] = false;

            if(!flag_sigle_stat[id-1])
            {
                return false;
            }
        } 
        else 
        {
            logger_->warn("control single aix time out  ! ID: ",id);
            std::cout << "[WARN]: control single aix time out  ! ID: " <<id <<std::endl;
            return false;
        }
    }

    return true;
}


//获取设备位置信息
bool Proto::Cmd_Get_Dev_Pos(tpDNPositionStruct & pos)
{
    if(Cmd_Get_RT_Infos())   //现获取增编，绝编，速度信息
    {
        if(_pc.Is_1st_GetPos())  //如果是第一次获取位置
        {
            pos = _pc.Refresh_Init_Pos(pos_rt_enc,abs_pos_rt_enc);
        }
        else     //如果不是第一次获取位置，或者运动中获取位置
        {
            pos = _pc.Refresh_Rt_Pos(pos_rt_enc,abs_pos_rt_enc);
        }

        return true;
    }
    else
    {
        return false;
    }


    // if(Cmd_Get_RT_Infos())
    // {
    //     pos.x = static_cast<float>(pos_rt_enc[0] / MM_2_ENC_HC);
    //     pos.y = static_cast<float>(pos_rt_enc[1] / MM_2_ENC_HC);
    //     pos.z = static_cast<float>(pos_rt_enc[2] / MM_2_ENC_HC);
    //     pos.rx = static_cast<float>(pos_rt_enc[3] / MM_2_ENC_HC);
    //     pos.ry = static_cast<float>(pos_rt_enc[4] / DEG_2_ENC_YY);
    //     pos.rz = static_cast<float>(pos_rt_enc[5] / DEG_2_ENC_YY);

    //     return true;
    // }
    // else
    // {
    //     return false;
    // }
    
}
//获取当前路劲点信息
bool Proto::Cmd_Get_CurPts_Infos(int & index, tpDNRouteStruct & route)
{
    if(Cmd_Get_RT_Infos())
    {
        if(cur_pts_idx > 0)
        index = cur_pts_idx - 1;

        try 
        {
            route = route_origin->at(index);
            // std::cout << "route_origin->at(3) = " << route_origin->at(3) << std::endl; // 输出
        } 
        catch (const std::out_of_range& e) 
        {
            std::cerr << "[Cmd_Get_CurPts_Infos] 错误: " << e.what() << std::endl;
            return false;
        }

        // route.pos.x = static_cast<float>(pos_rt_enc[0] / MM_2_ENC_HC);
        // route.pos.y = static_cast<float>(pos_rt_enc[1] / MM_2_ENC_HC);
        // route.pos.z = static_cast<float>(pos_rt_enc[2] / MM_2_ENC_HC);
        // route.pos.rx = static_cast<float>(pos_rt_enc[3] / MM_2_ENC_HC);
        // route.pos.ry = static_cast<float>(pos_rt_enc[4] / DEG_2_ENC_YY);
        // route.pos.rz = static_cast<float>(pos_rt_enc[5] / DEG_2_ENC_YY);

        // route.vx = static_cast<float>(spd_rt_enc[0] / MM_2_ENC_HC);
        // route.vy = static_cast<float>(spd_rt_enc[1] / MM_2_ENC_HC);
        // route.vz = static_cast<float>(spd_rt_enc[2] / MM_2_ENC_HC);
        // route.vrx = static_cast<float>(spd_rt_enc[3] / MM_2_ENC_HC);
        // route.vry = static_cast<float>(spd_rt_enc[4] / DEG_2_ENC_YY);
        // route.vrz = static_cast<float>(spd_rt_enc[5] / DEG_2_ENC_YY);
        
        return true;
    }
    else
    {
        return false;
    }
}


//设置坐标原点
int Proto::SetOrigin(int index, tpDNOriginEncoderStruct & encoder)
{
    uint32_t retry = 0;
    tpDNOriginEncoderStruct tmp_enc;
    tpDNPositionStruct pos;
    bool ret_get_pos = false;

    std::this_thread::sleep_for(std::chrono::milliseconds(800));  //给与下位机足够时间刷新 意优 和禾川的绝对值编码器值

    if(index == 0)   //以当前位置作为原点
    {
        while(++retry <= 10)
        {
            if(Cmd_Get_RT_Infos())  //现获取一下最新的绝对值
            {
                tmp_enc.ex = abs_pos_rt_enc[0];
                tmp_enc.ey = abs_pos_rt_enc[1];
                tmp_enc.ez_1 = abs_pos_rt_enc[2];
                tmp_enc.ez_2 = abs_pos_rt_enc[3];
                tmp_enc.erx = abs_pos_rt_enc[4];
                tmp_enc.erz = abs_pos_rt_enc[5];

                _pc.Reset_First_Get_Pos();   //将Is_First_Get_Pos 标记为置位false.下次再获取位置，会按绝对值编码器莱换算实际位置
                // return _pc.Set_OriPts(tmp_enc);
                if(_pc.Set_OriPts(tmp_enc) == TP_DN_STATUS_SUCCES)
                {
                    if(Cmd_Get_Dev_Pos(pos))
                    {
                        return TP_DN_STATUS_SUCCES;
                    }
                }
                return -3;
            }
            else
            {
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            }
        }

        if(retry > 10)
        { 
            logger_->warn("In Func SetOrigin ,Read RT infos time out!");
            std::cout << "[WARN]: In Func SetOrigin ,Read RT infos time out! " <<std::endl;
            return -2; 
        }
        
    }
    else if(index == 1)   //以输入坐标作为原点
    {
        _pc.Reset_First_Get_Pos();   //将Is_First_Get_Pos 标记为置位false.下次再获取位置，会按绝对值编码器莱换算实际位置
        // return _pc.Set_OriPts(encoder);
        // if(_pc.Set_OriPts(tmp_enc) == TP_DN_STATUS_SUCCES)
        if(_pc.Set_OriPts(encoder) == TP_DN_STATUS_SUCCES)
        {
            if(Cmd_Get_Dev_Pos(pos))
            {
                return TP_DN_STATUS_SUCCES;
            }
        }
        return -3;
        
    }
    else
    {
        return -1;
    }

    
}

//从本地文件 加载原点坐标
int Proto::LoadOrigin(void)
{
    return _pc.Load_Ori_Pts();
}

    //获取坐标原点的编码器值
int Proto::GetOriginEncoder(tpDNOriginEncoderStruct & encoder)
{
    encoder = _pc.Get_OriPts();
    return TP_DN_STATUS_SUCCES;
}


//判断是否运动任务结束
bool Proto::Is_Motion_Fin()
{
    bool ret;

    ret = Cmd_Get_RT_Infos();

    if(!ret)
    { return false; }

    return (is_pts_running == false);
}


bool Proto::Send_Process(uint8_t cmd,uint8_t * payload, uint32_t len_payload,bool need_ack)
{
    uint16_t retry = 0;
    uint32_t len_total;

    {
        std::unique_lock<std::mutex> lock(mutex_snd_process);  //发送流程互斥

        len_total = MakeTotalFrame(UPDOWN_T::TYPE_DOWN_STREAM, payload, len_payload); //组包发送

        flag_new_ack = false;

        while((retry <= FRAME_RETRY_SND) && (!flag_new_ack))
        {
            //-----实际发送------
            Send_Frame_To_Sock(len_total);  

            if(!need_ack)
            { return true; }

            //-----等待成功应答----  带超时的条件等待（最多等待5秒）
            {
                std::unique_lock<std::mutex> lock(mutex_ack);
                if (cv_ack.wait_for(lock, std::chrono::milliseconds(200), [this]{ return (flag_new_ack && (serial_num_ack == serial_num)); })) 
                {
                    // 条件被触发
                    // auto end = std::chrono::steady_clock::now();
                    // std::chrono::duration<double> elapsed = end - start;
                    // std::cout << "Thread A: Condition met after " 
                    //         << elapsed.count() << " seconds" << std::endl;
                    // flag_new_ack = false;
                    break;
                    // pts_num_snd = ((pts_num_total - pts_idx) < NUM_PTS_2_SENT) ? (pts_num_total - pts_idx) : NUM_PTS_2_SENT;  //下一个报文需要发送的点位数
                } 
                else 
                {
                    logger_->warn("cmd: {} send timeout !",cmd);
                    std::cout << "cmd: "<< cmd <<" send time out! " <<std::endl;
                    retry++;
                }
            }
        }

        flag_new_ack = false;
    }

    

    if(retry > FRAME_RETRY_SND)
    {
        return false;
    }
    else
    {
        return true;
    }
    
}



//发送路径点线程
bool Proto::Send_RoutePts_Loop()
{
    uint16_t retry = 0;
    uint32_t len_payload;
    uint32_t len_total;
    uint16_t pack_id;

    while(true)
    {
        {     //等待设置新的路径点
            std::unique_lock<std::mutex> lock(mutex_pts_snd);
            cv_pts_snd.wait(lock, [this] { return flag_new_route || !running_snd_frame; });

            flag_new_route = false;
            if (!running_snd_frame) 
            {
                return true;
            }
            
        }
        
        pack_id = 0; //分包id,主要用于调试打印
        retry = 0;
        pts_idx = 0;  //从第一个点位开始发送

        {
            std::unique_lock<std::mutex> lock(mutex_snd_process);  //发送流程互斥

            while(true)
            {
                pts_num_snd = (pts_num_total >= NUM_PTS_2_SENT)? NUM_PTS_2_SENT : pts_num_total;  //如果大于20个点暂定一次最多20个点
                
                flag_new_ack = false;
                retry = 0;
                pack_id++; //分包的id
                
                len_payload = Make_Pts_Mode1_PayloadFrame(pts_idx,pts_num_snd);  //发送路径点位 ，每次发送20个点
                len_total = MakeTotalFrame(UPDOWN_T::TYPE_DOWN_STREAM, payload_buff, len_payload); //组包发送

                while((retry <= FRAME_RETRY_SND) && (!flag_new_ack))
                {
                    //-----实际发送------
                    Send_Frame_To_Sock(len_total);  

                    //-----等待成功应答----  带超时的条件等待（最多等待5秒）
                    {
                        std::unique_lock<std::mutex> lock(mutex_ack);
                        if (cv_ack.wait_for(lock, std::chrono::milliseconds(500), [this]{ return (flag_new_ack && (serial_num_ack == serial_num)); })) 
                        {
                            // 条件被触发
                            // auto end = std::chrono::steady_clock::now();
                            // std::chrono::duration<double> elapsed = end - start;

                            pts_idx += pts_num_snd;   //索引后移
                            pts_num_total -= pts_num_snd;
                            if(pts_num_total == 0)
                            { break; }
                            // pts_num_snd = ((pts_num_total - pts_idx) < NUM_PTS_2_SENT) ? (pts_num_total - pts_idx) : NUM_PTS_2_SENT;  //下一个报文需要发送的点位数
                        } 
                        else 
                        {
                            // 超时发生
                            // auto end = std::chrono::steady_clock::now();
                            // std::chrono::duration<double> elapsed = end - start;
                            logger_->warn("Divide Pack: {0} Send routes points timeout!! retrytime:{1}",pack_id,retry);
                            std::cout << "Divide Pack: "<<pack_id <<" Send routes points timeout!! retrytime:"<< retry <<std::endl;
                            retry++;
                        }
                    }
                }
                
                if(retry > FRAME_RETRY_SND)
                {   
                    {
                        std::lock_guard<std::mutex> lock(mutex_pts_snd_finish);
                        is_pts_snd_timeout = true; //点位发送超时
                        is_pts_snd_finish = true;  //无论发送是否超时，结束标记都置位true
                    }
                    cv_pts_snd_finsh.notify_one(); //通知开始发送点位
                    logger_->warn("Send route points failed!! Abort at pack id: {0} retrytime: {1}",pack_id,retry);
                    std::cout << "Send route points failed!! Abort at pack id: "<<pack_id <<"retrytime: "<< retry <<std::endl;
                    break;  
                }

                if(0 == pts_num_total)  //点位全部发送完成
                {   
                    {
                        std::lock_guard<std::mutex> lock(mutex_pts_snd_finish);
                        is_pts_snd_timeout = false; //点位发送未超时，发送成功
                        is_pts_snd_finish = true; //无论发送是否超时，结束标记都置位true
                    }
                    cv_pts_snd_finsh.notify_one(); //通知开始发送点位
                    logger_->warn("send routes points Success ! Total pack : {}",pack_id);
                    std::cout << "send routes points Success ! Total pack :"<<pack_id  <<std::endl;
                    break;  
                }

            }
        }

    }


    return true;

}


//从第n个点开始组件报文，将连续num_pts个点组包,合成一个payload包
uint32_t Proto::Make_Pts_Mode1_PayloadFrame(uint32_t start_idx,uint32_t num_pts)
{
    int i;
    uint32_t idx = 0;

    //指令
    payload_buff[idx++] = CMD_PTS_MODE1; 
    //起始索引
    payload_buff[idx++] = (uint8_t)((start_idx & 0xFF00) >> 8);
    payload_buff[idx++] = (uint8_t)(start_idx & 0x00FF);
    //点数量
    payload_buff[idx++] = (uint8_t)((num_pts & 0xFF00) >> 8);
    payload_buff[idx++] = (uint8_t)(num_pts & 0x00FF);

    for(i = start_idx ; i < (start_idx+num_pts); i++)
    {
        const tpDNRouteStruct_Enc& elem = route_pts->at(i);
          
        payload_buff[idx++] = (uint8_t)((elem.pos.x & 0xFF000000) >> 24);
        payload_buff[idx++] = (uint8_t)((elem.pos.x & 0x00FF0000) >> 16);
        payload_buff[idx++] = (uint8_t)((elem.pos.x & 0x0000FF00) >> 8);
        payload_buff[idx++] = (uint8_t)(elem.pos.x & 0x000000FF);

        payload_buff[idx++] = (uint8_t)((elem.pos.y & 0xFF000000) >> 24);
        payload_buff[idx++] = (uint8_t)((elem.pos.y & 0x00FF0000) >> 16);
        payload_buff[idx++] = (uint8_t)((elem.pos.y & 0x0000FF00) >> 8);
        payload_buff[idx++] = (uint8_t)(elem.pos.y & 0x000000FF);

        payload_buff[idx++] = (uint8_t)((elem.pos.z_1 & 0xFF000000) >> 24);
        payload_buff[idx++] = (uint8_t)((elem.pos.z_1 & 0x00FF0000) >> 16);
        payload_buff[idx++] = (uint8_t)((elem.pos.z_1 & 0x0000FF00) >> 8);
        payload_buff[idx++] = (uint8_t)(elem.pos.z_1 & 0x000000FF);

        payload_buff[idx++] = (uint8_t)((elem.pos.z_2 & 0xFF000000) >> 24);
        payload_buff[idx++] = (uint8_t)((elem.pos.z_2 & 0x00FF0000) >> 16);
        payload_buff[idx++] = (uint8_t)((elem.pos.z_2 & 0x0000FF00) >> 8);
        payload_buff[idx++] = (uint8_t)(elem.pos.z_2 & 0x000000FF);

        payload_buff[idx++] = (uint8_t)((elem.pos.rx & 0xFF000000) >> 24);
        payload_buff[idx++] = (uint8_t)((elem.pos.rx & 0x00FF0000) >> 16);
        payload_buff[idx++] = (uint8_t)((elem.pos.rx & 0x0000FF00) >> 8);
        payload_buff[idx++] = (uint8_t)(elem.pos.rx & 0x000000FF);

        payload_buff[idx++] = (uint8_t)((elem.pos.rz & 0xFF000000) >> 24);
        payload_buff[idx++] = (uint8_t)((elem.pos.rz & 0x00FF0000) >> 16);
        payload_buff[idx++] = (uint8_t)((elem.pos.rz & 0x0000FF00) >> 8);
        payload_buff[idx++] = (uint8_t)(elem.pos.rz & 0x000000FF);


        // payload_buff[idx++] = (uint8_t)((elem.vx & 0xFF000000) >> 24);
        // payload_buff[idx++] = (uint8_t)((elem.vx & 0x00FF0000) >> 16);
        // payload_buff[idx++] = (uint8_t)((elem.vx & 0x0000FF00) >> 8);
        // payload_buff[idx++] = (uint8_t)(elem.vx & 0x000000FF);

        // payload_buff[idx++] = (uint8_t)((elem.vy & 0xFF000000) >> 24);
        // payload_buff[idx++] = (uint8_t)((elem.vy & 0x00FF0000) >> 16);
        // payload_buff[idx++] = (uint8_t)((elem.vy & 0x0000FF00) >> 8);
        // payload_buff[idx++] = (uint8_t)(elem.vy & 0x000000FF);

        // payload_buff[idx++] = (uint8_t)((elem.vz & 0xFF000000) >> 24);
        // payload_buff[idx++] = (uint8_t)((elem.vz & 0x00FF0000) >> 16);
        // payload_buff[idx++] = (uint8_t)((elem.vz & 0x0000FF00) >> 8);
        // payload_buff[idx++] = (uint8_t)(elem.vz & 0x000000FF);

        // payload_buff[idx++] = (uint8_t)((elem.vrx & 0xFF000000) >> 24);
        // payload_buff[idx++] = (uint8_t)((elem.vrx & 0x00FF0000) >> 16);
        // payload_buff[idx++] = (uint8_t)((elem.vrx & 0x0000FF00) >> 8);
        // payload_buff[idx++] = (uint8_t)(elem.vrx & 0x000000FF);

        // payload_buff[idx++] = (uint8_t)((elem.vry & 0xFF000000) >> 24);
        // payload_buff[idx++] = (uint8_t)((elem.vry & 0x00FF0000) >> 16);
        // payload_buff[idx++] = (uint8_t)((elem.vry & 0x0000FF00) >> 8);
        // payload_buff[idx++] = (uint8_t)(elem.vry & 0x000000FF);

        // payload_buff[idx++] = (uint8_t)((elem.vrz & 0xFF000000) >> 24);
        // payload_buff[idx++] = (uint8_t)((elem.vrz & 0x00FF0000) >> 16);
        // payload_buff[idx++] = (uint8_t)((elem.vrz & 0x0000FF00) >> 8);
        // payload_buff[idx++] = (uint8_t)(elem.vrz & 0x000000FF);
    }

    return idx;
} 

//将报文发送至sock
bool Proto::Send_Frame_To_Sock(uint16_t len)
{
    if(len > LEN_BUFF_SND)
    {return false;}

    tcp_socket_.send_data(reinterpret_cast<const char*>(frame_buff),len);

    return true;
}

//组成完成的帧
uint32_t Proto::MakeTotalFrame(UPDOWN_T updown,uint8_t * payload,uint16_t len_payload)
{
    
    uint32_t idx  =0;
    uint32_t i;
    uint16_t calc = 0;

    serial_num = 8; //DBG 
    // serial_num++;

    //帧头
    frame_buff[idx++] = HEAD_M;
    frame_buff[idx++] = HEAD_E;
    frame_buff[idx++] = HEAD_S1;
    frame_buff[idx++] = HEAD_S2;

    //协议版本
    frame_buff[idx++] = 1;  //协议版本暂定 1

    //报文序列号
    frame_buff[idx++] = (uint8_t)((serial_num & 0xFF000000) >> 24);
    frame_buff[idx++] = (uint8_t)((serial_num & 0x00FF0000) >> 16);
    frame_buff[idx++] = (uint8_t)((serial_num & 0x0000FF00) >> 8);
    frame_buff[idx++] = (uint8_t)(serial_num & 0x000000FF);

    //上下行类别
    frame_buff[idx++] = updown;

    //数据长度
    frame_buff[idx++] = (uint8_t)((len_payload & 0xFF00) >> 8);
    frame_buff[idx++] = (uint8_t)(len_payload & 0x00FF);

    //数据内容
    // for( i = 0 ; i < len_payload; i++)
    // {
    //     frame_buff[idx++] = payload[i];
    // }
    memcpy(&frame_buff[idx],payload,len_payload);
	idx += len_payload;

    // CRC16 
    calc = Crc16_Calc(frame_buff, idx) ;
    frame_buff[idx++] = (uint8_t)((calc & 0xFF00) >> 8);
    frame_buff[idx++] = (uint8_t)(calc & 0x00FF);

    //帧尾
    frame_buff[idx++] = TAIL_E;
    frame_buff[idx++] = TAIL_N;
    frame_buff[idx++] = TAIL_D;


    return idx;

}

void Proto::Cpy_2_PayloadBuff(uint32_t idx, uint32_t len)
{
    int i = 0;
    // int cpy_len = 0;

    // switch (cmd)
    // {
    // case CMD_ACK:   
    //     cpy_len = CMD_ACK_LEN_PAYLOAD;
    //     break;
    // case CMD_FB_INFO:
    //     cpy_len = CMD_FB_INFO_LEN_PAYLOAD;
    //     break;
    // case CMD_ORIGIN_ENC:
    //     cpy_len = CMD_ORIGIN_ENC_LEN_PAYLOAD;
    //     break;
    // default:
    //     break;
    // }

    for(int i = 0; i < len; i++)
    {
        payload_buffer_rx[i] = large_buffer_[idx];
        idx = (idx + 1) % LEN_LARGE_BUFFER;
    }
}


//解析接受到的报文
bool Proto::Parse_Frame(uint8_t * data, uint32_t len)
{
    uint16_t len_of_payload;
    uint8_t tmp_1,tmp_2;
    uint16_t crc_rcv,crc_calc;
    uint8_t cmd_type; //指令类型

    while (!ParseBuff_Is_Empty()) 
    {
        if(buff_count < MIN_FRAME_LEN)  //有效数据太少
        { break; }

        // 找到帧头
        if ((large_buffer_[buff_out] != HEAD_M)
               || (large_buffer_[(buff_out + 1) % LEN_LARGE_BUFFER] != HEAD_E)
               || (large_buffer_[(buff_out + 2) % LEN_LARGE_BUFFER] != HEAD_S1)
               || (large_buffer_[(buff_out + 3) % LEN_LARGE_BUFFER] != HEAD_S2))

         {
            large_buffer_[buff_out] = 0; //无效区域置0
            buff_out = (buff_out + 1) % LEN_LARGE_BUFFER;
            buff_count--;
            continue;  
         }

         //获取负载内容长度
        tmp_1 = large_buffer_[(buff_out + IDX_PAYLOAD_LEN) % LEN_LARGE_BUFFER];
        tmp_2 = large_buffer_[(buff_out + IDX_PAYLOAD_LEN+1) % LEN_LARGE_BUFFER];
        len_of_payload = static_cast<uint16_t>(tmp_1<<8) | tmp_2;

        // 找到帧尾
        if ((large_buffer_[(buff_out + IDX_TAIL_E(len_of_payload)) % LEN_LARGE_BUFFER] != TAIL_E)
               || (large_buffer_[(buff_out + IDX_TAIL_N(len_of_payload)) % LEN_LARGE_BUFFER] != TAIL_N)
               || (large_buffer_[(buff_out + IDX_TAIL_D(len_of_payload)) % LEN_LARGE_BUFFER] != TAIL_D))

         {
            large_buffer_[buff_out] = 0; //无效区域置0
            buff_out = (buff_out + 1) % LEN_LARGE_BUFFER;
            buff_count--;
            continue;  
         }

         //检查是否是下行报文
	    if(large_buffer_[buff_out + IDX_UPDOWN_TYPE] != TYPE_UP_STREAM)
        {
            large_buffer_[buff_out] = 0; //无效区域置0
            buff_out = (buff_out + 1) % LEN_LARGE_BUFFER;
            buff_count--;
            continue;  
         }

         // Crc16校验
         tmp_1 = large_buffer_[(buff_out + IDX_CRC_H(len_of_payload)) % LEN_LARGE_BUFFER];
         tmp_2 = large_buffer_[(buff_out + IDX_CRC_L(len_of_payload)) % LEN_LARGE_BUFFER];

         crc_rcv = static_cast<uint16_t>(tmp_1 << 8) | tmp_2;
         crc_calc = Crc16_Calc(&large_buffer_[buff_out], (12 + len_of_payload));
         if(crc_calc != crc_rcv)
         {
            large_buffer_[buff_out] = 0; //无效区域置0
            buff_out = (buff_out + 1) % LEN_LARGE_BUFFER;
            buff_count--;
            continue;  
         }

         //----校验通过------
         
         //-------执行具体的解析方法-------
        cmd_type = large_buffer_[(buff_out + IDX_PAYLOAD_CMD) % LEN_LARGE_BUFFER];

        Cpy_2_PayloadBuff((buff_out + IDX_PAYLOAD_CMD) % LEN_LARGE_BUFFER, len_of_payload);
        // Call_ParseFunc(cmd_type,&large_buffer_[(buff_out + IDX_PAYLOAD_CMD) % LEN_LARGE_BUFFER],len_of_payload);
        Call_ParseFunc(cmd_type,payload_buffer_rx,len_of_payload);

        //-----刷新缓冲区剩余有效字节数，和buff_out的值--------
        buff_count = buff_count - FRAME_TOTAL_WITHOUT_PAYLOAD - len_of_payload;
        buff_out = (buff_out + FRAME_TOTAL_WITHOUT_PAYLOAD + len_of_payload) % LEN_LARGE_BUFFER;

    }

    return false;

}


bool Proto::Parse_Ack(uint8_t * data, int len)  //解析应答
{
    uint8_t cmd;
    uint16_t ack_code ;

    cmd = data[0];

    if(!(cmd & 0x80))
    {   return false;   }

    ack_code = (uint16_t)(data[1] << 8) | data[2];

    serial_num_ack = (uint32_t)(data[3] << 24) | (data[4] << 16) | (data[5] << 8) | data[6];

    if(serial_num_ack != serial_num)
    { return false;}

    Event_Get_Ack();

    return true;
}


bool Proto::Parse_RealTime(uint8_t * data, int len)  //解析实时值
{
    uint8_t i = 0;
    uint16_t idx = 0;
    uint8_t is_trigger = false;
    // uint8_t tmp_data[128] = {0};

    // memcpy(tmp_data,data,len);

    idx = 1;
    // cur_pts_idx =  (uint16_t)(data[1] << 8) | data[2];  //当前路径点索引
    // cur_task_total_pts = (uint16_t)(data[3] << 8) | data[4]; //当前路径点任务剩余点数
    // is_pts_running = data[5];                            //是否路径点正在执行

    // for(i = 0; i < 6; i++)
    // {
    //     pos_rt_enc[i] = (int32_t)(data[6 + (i * 8)] << 24) | (data[7 + (i * 8)] << 24) | (data[8+ (i * 8)] << 24) | data[9+ (i * 8)];
    //     spd_rt_enc[i] = (int32_t)(data[10+ (i * 8)] << 24) | (data[11+ (i * 8)] << 24) | (data[12+ (i * 8)] << 24) | data[13+ (i * 8)];
    // }
    cur_pts_idx =  (uint16_t)(data[idx++] << 8) | data[idx++];  //当前路径点索引
    cur_task_total_pts = (uint16_t)(data[idx++] << 8) | data[idx++]; //当前路径点任务剩余点数
    is_pts_running = data[idx++];          

    is_trigger =  data[idx++];   //该报文上传原因是否是因为trigger触发
    trigger_num = (uint32_t)(data[idx++] << 24) | (data[idx++] << 16) | (data[idx++] << 8) | data[idx++];   //控制板接收到的Trigger的数量
    
    for(i = 0; i < 6; i++)
    {
        pos_rt_enc[i]     = (int32_t)(data[idx++] << 24) | (data[idx++] << 16) | (data[idx++] << 8) | data[idx++];
        spd_rt_enc[i]     = (int32_t)(data[idx++] << 24) | (data[idx++] << 16) | (data[idx++] << 8) | data[idx++];
        abs_pos_rt_enc[i] = (int32_t)(data[idx++] << 24) | (data[idx++] << 16) | (data[idx++] << 8) | data[idx++];
    }

    logger_->debug("RealTime --- is_pts_run: {0},cur_pts_idx: {1} ,Pos_1: {2},Pos_2: {3},Pos_3: {4},Pos_4: {5},Pos_5: {6},Pos_6: {7},Abs_1: {8},Abs_2: {9},Abs_3: {10},Abs_4: {11},Abs_5: {12},Abs_6: {13}",is_pts_running,cur_pts_idx,pos_rt_enc[0],pos_rt_enc[1],pos_rt_enc[2],pos_rt_enc[3],pos_rt_enc[4],pos_rt_enc[5],abs_pos_rt_enc[0],abs_pos_rt_enc[1],abs_pos_rt_enc[2],abs_pos_rt_enc[3],abs_pos_rt_enc[4],abs_pos_rt_enc[5]);
    std::cout << "RealTime ---"<< " is_pts_run: " <<is_pts_running<< " cur_pts_idx: "<< cur_pts_idx <<" Pos_1:"<< pos_rt_enc[0]<<" Pos_2:"<< pos_rt_enc[1]<<" Pos_3:"<< pos_rt_enc[2]<<" Pos_4:"<< pos_rt_enc[3]<<" Pos_5:"<< pos_rt_enc[4]<<" Pos_6:"<< pos_rt_enc[5] << "Abs_1:"<< abs_pos_rt_enc[0]<<" Abs_2:"<< abs_pos_rt_enc[1]<<" Abs_3:"<< abs_pos_rt_enc[2]<<" Abs_4:"<< abs_pos_rt_enc[3]<<" Abs_5:"<< abs_pos_rt_enc[4]<<" Abs_6:"<< abs_pos_rt_enc[5]<<std::endl;

    if(is_trigger == 0)
    {
        Event_Get_Rt_Infos();   //此种情况是对读取实时信息的应答
    }
    else
    {
        tpDNRouteStruct tmp_toute;
        //此种情况是控制板发生了 Trigger，需要调用回调
        if(_pc.Is_1st_GetPos())  //如果是第一次获取位置
        {
            _pc.Refresh_Init_Pos(pos_rt_enc,abs_pos_rt_enc);
        }
        else     //如果不是第一次获取位置，或者运动中获取位置
        {
            _pc.Refresh_Rt_Pos(pos_rt_enc,abs_pos_rt_enc);
        }

        auto result = _pc.Get_Pos();
        if(!result.first)
        {
            return false;
        }

        tmp_toute.pos = result.second;

        TriggerCallback(cur_task_total_pts, cur_pts_idx, trigger_num, tmp_toute);
    }
    

    return true;
}

bool Proto::Parse_OriginPt_Enc(uint8_t * data, int len)  //解析原点编码器
{
    return true;
}

bool Proto::Parse_Drv_Enable_Stat(uint8_t * data, int len)  //解析驱动器使能状态
{
    // //------4个禾川+1个意优
    // if((data[1] == 10)
    //  && (data[2] == 10)
    //  && (data[3] == 10)
    //  && (data[4] == 10)
    //  && (data[5] == 10))
    //  {
    //     is_all_drv_enable = true;
    //  }
    // ------4个禾川+2个意优
    if((data[1] == 10)
     && (data[2] == 10)
     && (data[3] == 10)
     && (data[4] == 10)
     && (data[5] == 10)
     && (data[6] == 10))
     {
        is_all_drv_enable = true;
     }

    return true;
}

bool Proto::Parse_Motion_Fin(uint8_t * data, int len)  //解析运动结束报文
{

    is_drv_motion_fin = true;

    pos_rt_enc[0] = (int32_t)(data[0] << 24) | (data[1] << 24) | (data[2] << 24) | data[3];
    pos_rt_enc[1] = (int32_t)(data[4] << 24) | (data[5] << 24) | (data[6] << 24) | data[7];
    pos_rt_enc[2] = (int32_t)(data[8] << 24) | (data[9] << 24) | (data[10] << 24) | data[11];
    pos_rt_enc[3] = (int32_t)(data[12] << 24) | (data[13] << 24) | (data[14] << 24) | data[15];
    pos_rt_enc[4] = (int32_t)(data[16] << 24) | (data[17] << 24) | (data[18] << 24) | data[19];
    pos_rt_enc[5] = (int32_t)(data[20] << 24) | (data[21] << 24) | (data[22] << 24) | data[23];

    return true;
}

bool Proto::Parse_SingleAix_Fin(uint8_t * data, int len)  //单轴控制完成
{

    uint8_t id;
    uint8_t stat;

    id = data[1];
    stat = data[2];
    pos_rt_enc[id-1] = (int32_t)(data[3] << 24) | (data[4] << 24) | (data[5] << 24) | data[6];

    Event_Get_SingleAix_Fin(id,stat);

    return true;
}

bool Proto::Parse_Motor_Err(uint8_t * data, int len)  //单轴控制完成
{

    motor_errcode[0] = (uint16_t)(data[1] << 8) | data[2];
    motor_errcode[1] = (uint16_t)(data[3] << 8) | data[4];
    motor_errcode[2] = (uint16_t)(data[5] << 8) | data[6];
    motor_errcode[3] = (uint16_t)(data[7] << 8) | data[8];
    motor_errcode[4] = (uint16_t)(data[9] << 8) | data[10];
    motor_errcode[5] = (uint16_t)(data[11] << 8) | data[12];

    MotorErrCallback(motor_errcode);

    return true;
}

bool Proto::Parse_Set_Default_Spd_Fin(uint8_t * data, int len)  //解析设置默认速度
{
    uint8_t id;

    id = data[1];

    if(data[2])
    {
        is_set_default_spd_ok[id-1] = true;
    }
    else
    {
        is_set_default_spd_ok[id-1] = false;
    }

    Event_Ack_GetSetSpd();
    
    return true;
}

bool Proto::Parse_Get_Default_Spd_Fin(uint8_t * data, int len)  //解析获取默认速度
{
    uint8_t id;
    
    id = data[1];

    if(data[2])
    {
        is_get_default_spd_ok[id-1] = true;
        default_spd_Param[id-1]= (int)(data[3] << 24) | (data[4] << 16) | ( data[5] <<8 ) | data[6] ;
    }
    else
    {
        is_get_default_spd_ok[id-1] = false;
    }

    Event_Ack_GetSetSpd();

    return true;
}

void Proto::parse_loop() 
{
    char* buff_ptr = NULL;
    int len = 0;
    int i = 0;

    while(running_parse) 
    {
        buff_ptr = NULL;
        len = 0;

        buff_ptr = tcp_socket_.wait_and_fetch_data(&len);
        if (len <= 0) 
        { break;  } // socket关闭或出错
        {
            std::lock_guard<std::mutex> lock(mutex_largebuff);

            for( i = 0; i < len; i++)
            {
                if(ParseBuff_Is_Full())   //接收缓冲区满
                {  break; }   

                large_buffer_[buff_in] = buff_ptr[i];
                buff_in++;
                if(buff_in == LEN_LARGE_BUFFER)
                {   buff_in = 0; }

                buff_count++;
            }

            len = 0;
            
            // // 处理缓冲区循环覆盖
            // if(buffer_index_ + len > sizeof(large_buffer_)) 
            // {
            //     buffer_index_ = 0; // 重置位置
            // }
            // else
            // {
            //     memcpy(large_buffer_ + buffer_index_, buff_ptr, len);
            //     buffer_index_ += len;
            // }
            
        }

        //实际解析
        Parse_Frame(large_buffer_, len);
        
    }
}





